#include "WPILib.h"
#include "math.h"
#include "DriveTrain.h"
#include "Shooter.h"


class RobotDemo : public SimpleRobot
{
	Joystick *driveJoy;
	DriveTrain *drive;
	Shooter *shoot;
public:
	RobotDemo(void)
	{
		driveJoy = new Joystick(1); //Joystick with driving controls
		drive = new DriveTrain();   //All the drive related stuff including pids and dead reckoning
		shoot = new Shooter();      //All the shooter related stuff
	}
	

	void Autonomous(void)
	{
		//Set everything up//
		drive->AutoStart();
		drive->DeadStart();
		
		//Drive forward five feet//
		drive->AutoStraight(5);
		
		//Stop Dead reckoning tracking//
		drive->DeadStop();
	}

	void OperatorControl(void)
	{
		//Set everything up//
		drive->OpStart();
		shoot->Start();
		drive->DeadStart();

		while (IsOperatorControl())
		{
			//Controls//
			drive->OpControl(driveJoy->GetX(), driveJoy->GetY());
			shoot->OpControl(driveJoy->GetRawButton(1), driveJoy->GetRawAxis(3), driveJoy->GetRawButton(3));
			
			//Updating the dead reckoning display//
			drive->DeadUpdate();
			drive->DeadDisplay();
			
			Wait(.005);
		}
	}
	
	
};

START_ROBOT_CLASS(RobotDemo);

